#pragma once

#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Core>



enum camera_modul{
    pinHole_modul,
    fishEye_modul
};

class camera{
public:
	camera(){};
    ~camera(){};
};


// 针孔相机模型
class pinHole:public camera
{
private:
    
public:
    camera_modul modul = pinHole_modul;
    // 图像宽高
    int pic_row = 480;
    int pic_col = 640;

    cv::Mat mask;// 图像
    // 内部参数
    double m_k1 = 1.25323e-01;// 畸变参数
    double m_k2 = -2.51452e-01;
    double m_p1 = 7.12e-04;
    double m_p2 = 6.217e-03;

    double fx = 6.165911254882812e+02;// 焦距参数
    double fy = 6.166796264648438e+02;
    double cx = 3.242193603515625e+02;
    double cy = 2.3942701721191406e+02;

    pinHole(/* args */){
        
    }
    ~pinHole(){}
    // 这个地方有个问题是利用检测出来的特征点是行列的 实际上对应的是 y和x系列的数据，因此还要转换一下
    // 使用最简单的无畸变模型
    void Img2Normalization(Eigen::Vector2d & in, Eigen::Vector2d & out){
        double m_inv_K11 = 1.0 / fx;
        double m_inv_K13 = -cx / fx;
        double m_inv_K22 = 1.0 / fy;
        double m_inv_K23 = -cy / fy;
        
        double mx_d = m_inv_K11 * in[0] + m_inv_K13;
        double my_d = m_inv_K22 * in[1] + m_inv_K23;
        
        out << mx_d, my_d;
    };

    // 把归一化平面的点映射到图像坐标系下
    void Normalization2Img(Eigen::Vector2d & in, Eigen::Vector2d & out){
        double u = fx * in[0] + cx;
        double v = fy * in[1] + cy;
        out << u, v;
    }
    void Normalization2Img(Eigen::Vector3d & in, Eigen::Vector3d & out){
        double u = fx * in[0]/in[2] + cx;
        double v = fy * in[1]/in[2] + cy;
        out << u, v, -1.;
    }
};

